#ifndef MANIPULATOR_H
#define MANIPULATOR_H

#include <iostream>
#include <stdio.h>
#include <fstream>
#include <math.h>
#include <vector>
#include "serialstd.hpp"
using namespace std;

class Manipulator
{
    private:

    enum EReadResult { RDONE , RUPDATE , RUNKNOWN};
    // manipulator frame has origin at the shoulder, zeroed at the undeployed position
    float S; // angle of shoulder joint [rad] in manipulator frame
    float E; // angle of elbow joint [rad]
    float W; // angle of wrist joint [rad] - currently not implemented! Set to zero!
    float V; // position of vertical actuator [mm]
    float G; // gripper opening [rad]

    float X; // tip position in x-direction in man. frame [mm]
    float Y; // tip position in y-direction in man. frame [mm]
    float Z; // tip position in z-direction in man. frame [mm]
    float C; // tip angle relative to S = 0 angle [deg]

	float Smove; // S originally commanded
	float Emove;
	float Wmove;
	float Vmove;
	float Gmove;
	//float Spred; // predicted

	// past state variables for history
	float S_OLD;
	float E_OLD;
	float W_OLD;
	float V_OLD;

	// needed to implement a time history of state variables
	vector<float> SV; // vector of S
	vector<float> EV; // vector of E
	vector<float> WV;
	vector<float> VV;
	vector<float> GV;

	vector<float> XV;
	vector<float> YV;
	vector<float> ZV;
	vector<float> CV;

    static const float Z0=140.0;
    static const float L01=120.0;   // length of shoulder-elbow link [mm]
    static const float L02=30.0; // length of elbow-wrist link [mm]
    static const float L1=150.0, ; // length of wrist-end-effector link [mm]
    static const float L2=70.0;
    static const float L3=60.0;
    static const float L4=90.0;
    static const float L5=60.0;

	// joint limits - in int format for "rounder" comparison
    static const int sMax = 90;
    static const int sMin = -90;
    static const int eMax = 115;
    static const int eMin = -115;
    static const int wMax = 90;
    static const int wMin = -90;
    static const int vMax = 140;
    static const int vMin = 0.0;
    static const int gMax = 140;
    static const int gMin = 80;

    void endTransform(float[][4], float, float, float, float );
//void endTransform(vector< vector<float> >, float, float, float, float );
    void Jacobian(float[][4],float,float,float,float);
    void JacobianNoOrient(float[][3],float,float,float,float);

    //void JacobianT(float[][2],float,float,float,float);
	void WeightMatrix(float[][3], float, float, float, float, float, float, float, float) ;
	float wrapAngle(float);
	float fMod(float, float);
	void addStateToVector();
    int findMinVector(vector<float>);

    void writeMicroAction(float , float , float , float );
    EReadResult readMicro(char*);
    char convertangle(float);
    void writeMicroGetState();
    void writeMicroGripper(float);


	int stateChangeFlag;
	int exceedLimitsFlag;

    public:

    bool displayOn;

	//temporarz state variables for the Set
    float xS; float yS; float zS; float cS;
    float sS; float eS; float vS; float wS; int gS;

    enum ESetResult { POSE, JOINTS, GRIPPER, NONE ,UNKNOWN };

    Manipulator(float,float,float,float,float); // constructor

     //~Manipulator(); // destructor
    void setJoints(float,float,float,float);
    void setPose(float,float,float,float);
    void setGripper(float);
    void Set(float,float,float,float,  float, float, float, float,float);
    ESetResult SetElement(float, int);
    void doSet(ESetResult);

    float *Get();
    float *getJoints();
    float *getPose();
    float getX();
    float getY();
    float getZ();
    float getC();

    float getS();
    float getE();
    float getW();
    float getV();
    float getG();
    int checkLimits(float,float,float,float,float,float,float,float,float,bool);
	float *dampedLeastSquaresNoOrient(float[][3], float, float,float);
	float *dampedLeastSquares(float[][4], float, float,float,float);
    float *fwdKinematics(float,float,float,float);
    float *invKinematicsAlgebraic(float,float,float,float);
	int findMinArray(float[], int);
    float *invKinematics(float,float,float,float);
    //float *incrementalPose(float,float,float,float);
    void writeMicroReset();

    //change USB device
    void setUSB(std::string);
    void updateState(bool);
    int actionExecuted();
};

#endif // MANIPULATOR_H_INCLUDED
